#include "chassis_base.h"

#if MOTOR_CAN_ENABLE == 1

#include "../algorithm/filter.h"
#include "motor_ctrl.h"
//#include "drv_BrcBd.h"
#include "stdbool.h"

#include <math.h>
#include <string.h>

extern CAN_HandleTypeDef CHASSIS_MOTORS_HCAN;
extern CAN_HandleTypeDef STEERING_MOTORS_HCAN;
extern TIM_HandleTypeDef CHASSIS_MOTORS_TIM;

int streeingZero[4] = {200, 270, 100, 750 };
int streeingFdb_data[4];
float steering_output[4];
float total_output;

//inline float fabsf(float t){return (t>0)?t:(-t);}
//inline int16_t abs(int16_t t){return (t>0)?t:(-t);}

/**
  * @brief mecanum forward calcuation
  * @param[in]  s_wheel     pointer to chassis_wheels_state_t (4 wheel rpm)
  * @return chassis_state_t including v_x,v_y,w_z
  */
/*麦克纳姆轮运动学正向解算，即从四轮轮速换算到整车速度*/
chassis_state_t mecanum_forward(chassis_wheels_state_t* s_wheel){
	chassis_state_t s_chassis;
	s_chassis.v_x = C_WHEEL*(s_wheel->M_LF - s_wheel->M_RF + s_wheel->M_LB - s_wheel->M_RB)/4 /60;
	s_chassis.v_y = C_WHEEL*(KXY_BACK*s_wheel->M_RF + KXY_BACK*s_wheel->M_LF
					-KXY_FRONT*s_wheel->M_LB - KXY_FRONT*s_wheel->M_RB)/(2*(KXY_BACK+KXY_FRONT))/60;
	s_chassis.w_z = (D_WHEEL/2)*(s_wheel->M_RF + s_wheel->M_LF + s_wheel->M_LB + s_wheel->M_RB)/(2*(KXY_BACK+KXY_FRONT));
	return s_chassis;
}

/**
  * @brief mecanum reverse calcuation
  * @param[in]  s_chassis    pointer to chassis_state_t (v_x,v_y,w_z)
  * @return chassis_wheels_state_t (4 wheels speed)
  */
/*麦克纳姆轮运动学逆向解算，即从整车速度换算到四轮轮速*/
chassis_wheels_state_t mecanum_reverse(chassis_state_t* s_chassis){
	chassis_wheels_state_t s_wheel;
	s_wheel.M_RF = (60/C_WHEEL)*(-s_chassis->v_x + s_chassis->v_y) + KXY_FRONT/(D_WHEEL/2) * s_chassis->w_z;
	s_wheel.M_LF = (60/C_WHEEL)*( s_chassis->v_x + s_chassis->v_y) + KXY_FRONT/(D_WHEEL/2) * s_chassis->w_z;
	s_wheel.M_LB = (60/C_WHEEL)*( s_chassis->v_x - s_chassis->v_y) + KXY_BACK/(D_WHEEL/2) *  s_chassis->w_z;
	s_wheel.M_RB = (60/C_WHEEL)*(-s_chassis->v_x - s_chassis->v_y) + KXY_BACK/(D_WHEEL/2) *  s_chassis->w_z;

	return s_wheel;
}
/*全向轮运动学正向解算，即从四轮轮速换算到整车速度*/
chassis_state_t omni_forward(chassis_wheels_state_t* s_wheel)//s_wheel是rpm，3508的rpm要考虑减速比：REDUCTION_RATIO
{
	chassis_state_t s_chassis;
	s_chassis.v_x = C_WHEEL*( s_wheel->M_LF - s_wheel->M_RF + s_wheel->M_LB - s_wheel->M_RB)/sqrt(2)/4 /60;
	s_chassis.v_y = C_WHEEL*( s_wheel->M_LF + s_wheel->M_RF - s_wheel->M_LB - s_wheel->M_RB)/sqrt(2)/4 /60;//四轮转速平均	
	s_chassis.w_z = (D_WHEEL/2)*( s_wheel->M_RF + s_wheel->M_LF + s_wheel->M_LB + s_wheel->M_RB)/4/K_R;
	return s_chassis;
}

/*全向轮运动学逆向解算，即从整车速度换算到四轮轮速*/
chassis_wheels_state_t omni_reverse(chassis_state_t* s_chassis)
{
	chassis_wheels_state_t s_wheel;
		s_wheel.M_RF = (60/C_WHEEL)*(-s_chassis->v_x + s_chassis->v_y) * sqrt(2) + K_R/(D_WHEEL/2) * s_chassis->w_z;
		s_wheel.M_LF = (60/C_WHEEL)*( s_chassis->v_x + s_chassis->v_y) * sqrt(2) + K_R/(D_WHEEL/2) * s_chassis->w_z;
		s_wheel.M_LB = (60/C_WHEEL)*( s_chassis->v_x - s_chassis->v_y) * sqrt(2) + K_R/(D_WHEEL/2) * s_chassis->w_z;
		s_wheel.M_RB = (60/C_WHEEL)*(-s_chassis->v_x - s_chassis->v_y) * sqrt(2) + K_R/(D_WHEEL/2) * s_chassis->w_z;
	return s_wheel;
}

float my_sqrt(float number)
{
	long i;
	float xx, yy;
	const float f = 1.5F;
	xx = number * 0.5F;
	yy = number;
	i = * ( long * ) &yy;
	i = 0x5f375a86 - ( i >> 1 );

	yy = * ( float * ) &i;
	yy = yy * ( f - ( xx * yy * yy ) );
	yy = yy * ( f - ( xx * yy * yy ) );
	return number * yy;
}
float my_pow(float a)
{
	return a*a;
}
	chassis_wheels_state_t s_wheel;
calculate_para basePara;

chassis_wheels_state_t steeringMotor_reverse(chassis_state_t* s_chassis)
{

	
steering_wheel_speed w_speed;


	
	
	if (s_chassis->v_x == 0 && s_chassis->v_y == 0 && s_chassis->w_z == 0) //当三个速度向量均为0时则不进行运算
	{
		s_wheel.M_LF = 0; //当不进行计算时，所有的轮速均为0 轮子角度保持上一帧的数据不变
		s_wheel.M_RF = 0;
		s_wheel.M_LB = 0;
		s_wheel.M_RB = 0;
		w_speed.M_LF= 0;
		w_speed.M_RF = 0;
		w_speed.M_LB = 0;
		w_speed.M_RB= 0;
	}
	else
	{
	basePara.alpha = atan2(s_chassis->v_y, s_chassis->v_x); //计算车体偏航角   取值范围（-pi -- pi）
		basePara.sinAlpha = sinf(basePara.alpha); //（-1 --  1）
		basePara.cosAlpha = cosf(basePara.alpha); //（-1 --  1）
		basePara.speedV = my_sqrt(my_pow(s_chassis->v_y) + my_pow(s_chassis->v_x));
	//LF:1
w_speed.M_LF = my_sqrt((my_pow(W_CDISTANCE) * my_pow(s_chassis->w_z)) / 2 + my_pow(basePara.speedV) 
    + (WHEELBASE * basePara.sinAlpha + WHEELTRACK * basePara.cosAlpha) * s_chassis->w_z* basePara.speedV);
s_wheel.DIR_LF = (PI / 2) - atan2((2 * basePara.speedV * basePara.cosAlpha
    + WHEELTRACK * s_chassis->w_z), (2 * basePara.speedV * basePara.sinAlpha + WHEELBASE * s_chassis->w_z));
	if (s_wheel.DIR_LF < 0)
		{
			s_wheel.DIR_LF += 2 * PI;
		}
		s_wheel.DIR_LF = s_wheel.DIR_LF * RADIAN_TO_DEGREE; //(0 -- 360)
	//LB:2
w_speed.M_LB = my_sqrt((my_pow(W_CDISTANCE) * my_pow(s_chassis->w_z)) / 2 + my_pow(basePara.speedV) 
    + (-WHEELBASE * basePara.sinAlpha + WHEELTRACK * basePara.cosAlpha) * s_chassis->w_z* basePara.speedV);
s_wheel.DIR_LB = (PI / 2) - atan2((2 * basePara.speedV * basePara.cosAlpha
    + WHEELTRACK * s_chassis->w_z), -(-2 * basePara.speedV * basePara.sinAlpha + WHEELBASE * s_chassis->w_z));
		if (s_wheel.DIR_LB < 0)
		{
			s_wheel.DIR_LB += 2 * PI;
		}
		s_wheel.DIR_LB = s_wheel.DIR_LB * RADIAN_TO_DEGREE; //(0 -- 360)
		//RF:0
w_speed.M_RF = my_sqrt((my_pow(W_CDISTANCE) * my_pow(s_chassis->w_z)) / 2 + my_pow(basePara.speedV) 
    + (WHEELBASE * basePara.sinAlpha - WHEELTRACK * basePara.cosAlpha) * s_chassis->w_z* basePara.speedV);
s_wheel.DIR_RF = (PI / 2) - atan2((2 * basePara.speedV * basePara.cosAlpha
    - WHEELTRACK * s_chassis->w_z), (2 * basePara.speedV * basePara.sinAlpha + WHEELBASE * s_chassis->w_z));
	if (s_wheel.DIR_RF < 0)
		{
			s_wheel.DIR_RF += 2 * PI;
		}
		s_wheel.DIR_RF = s_wheel.DIR_RF * RADIAN_TO_DEGREE; //(0 -- 360)
	//RB:3
w_speed.M_RB = my_sqrt((my_pow(W_CDISTANCE) * my_pow(s_chassis->w_z)) / 2 + my_pow(basePara.speedV) 
    + (-WHEELBASE * basePara.sinAlpha - WHEELTRACK * basePara.cosAlpha) * s_chassis->w_z* basePara.speedV);
s_wheel.DIR_RB = (PI / 2) - atan2((2 * basePara.speedV * basePara.cosAlpha
    - WHEELTRACK * s_chassis->w_z), -(-2 * basePara.speedV * basePara.sinAlpha + WHEELBASE * s_chassis->w_z));
		if (s_wheel.DIR_RB < 0)
		{
			s_wheel.DIR_RB += 2 * PI;
		}
		s_wheel.DIR_RB = s_wheel.DIR_RB * RADIAN_TO_DEGREE; //(0 -- 360)
		
		
		
		s_wheel.M_LF = -w_speed.M_LF / WHEEL_PERIMETER *60 / REDUCTION_RATIO; //mm/s --->n/min 转子转速
	  s_wheel.M_LB = w_speed.M_LB / WHEEL_PERIMETER * 60 / REDUCTION_RATIO;
	  s_wheel.M_RF = w_speed.M_RF / WHEEL_PERIMETER * 60  / REDUCTION_RATIO;
	  s_wheel.M_RB = -w_speed.M_RB / WHEEL_PERIMETER * 60  / REDUCTION_RATIO;
		
	}	
	return s_wheel;		
}


///////修改待测试
//chassis_state_t steering_forward(chassis_wheels_state_t* s_wheel){
//	chassis_state_t s_chassis;
//	steering_wheel_speed w_speed;
////	s_chassis.v_x = WHEEL_PERIMETER*(w_speed.M_LF + w_speed.M_RF + w_speed.M_LB + w_speed.M_RB)/4 /60;
////	s_chassis.v_y = WHEEL_PERIMETER*(w_speed.M_LF + w_speed.M_RF + w_speed.M_LB + w_speed.M_RB)/4 /60;
////	s_chassis.w_z = (WHEEL_DIAMETER/2)*(w_speed.M_RF + w_speed.M_LF + w_speed.M_LB + w_speed.M_RB)/(2*(KXY_BACK+KXY_FRONT));
//	
//	// 将方向角转换为弧度制
//  s_wheel->DIR_LF = s_wheel->DIR_LF * DEGREE_TO_RADIAN;
//	s_wheel->DIR_LB = s_wheel->DIR_LB * DEGREE_TO_RADIAN;
//  s_wheel->DIR_RF = s_wheel->DIR_RF * DEGREE_TO_RADIAN;
//	s_wheel->DIR_RB = s_wheel->DIR_RB * DEGREE_TO_RADIAN;
//    // 计算车辆在x轴和y轴方向上的速度分量
//     s_chassis.v_x =WHEEL_PERIMETER*(w_speed.M_LF + w_speed.M_RF + w_speed.M_LB + w_speed.M_RB)/4 /60* (cos(s_wheel->DIR_LF)+cos(s_wheel->DIR_LB)+cos(s_wheel->DIR_RF)+cos(s_wheel->DIR_RB))/4;
//     s_chassis.v_y  = WHEEL_PERIMETER*(w_speed.M_LF + w_speed.M_RF + w_speed.M_LB + w_speed.M_RB)/4 /60*(sin(s_wheel->DIR_LF)+sin(s_wheel->DIR_LB)+sin(s_wheel->DIR_RF)+sin(s_wheel->DIR_RB))/4;

//    // 计算车辆z轴的自转速度
//    s_chassis.w_z = (1 / WHEELBASE) * ((s_chassis.v_y * cos(s_wheel->DIR_LF) -  s_chassis.v_x * sin(s_wheel->DIR_LF))+(s_chassis.v_y * cos(s_wheel->DIR_LB) -  s_chassis.v_x * sin(s_wheel->DIR_LB))
//	+(s_chassis.v_y * cos(s_wheel->DIR_RF) -  s_chassis.v_x * sin(s_wheel->DIR_RF))+(s_chassis.v_y * cos(s_wheel->DIR_RB) -  s_chassis.v_x * sin(s_wheel->DIR_RB)))/4;
//	return s_chassis;
//}		


#if USE_SWERVE_CHASSIS == 1
typedef struct{ float x; float y; }vector_t;
//舵轮的实验性代码
void swerve_reverse(chassis_state_t* s_chassis, chassis_wheels_state_t* s_wheel, chassis_wheels_dir_t* s_dir){
	vector_t vec_rf,vec_lf,vec_lb,vec_rb;
	vector_t vec_wz;
	vec_wz.x = K_SWERVE*cosf(SWERVE_SPIN_ANG*PI/180) * s_chassis->w_z;
	vec_wz.y = K_SWERVE*sinf(SWERVE_SPIN_ANG*PI/180) * s_chassis->w_z;


	vec_rf.x = s_chassis->v_x - vec_wz.x;
	vec_rf.y = s_chassis->v_y - vec_wz.y;

	vec_lf.x = s_chassis->v_x + vec_wz.x;
	vec_lf.y = s_chassis->v_y - vec_wz.y;

	vec_lb.x = s_chassis->v_x + vec_wz.x;
	vec_lb.y = s_chassis->v_y + vec_wz.y;

	vec_rb.x = s_chassis->v_x - vec_wz.x;
	vec_rb.y = s_chassis->v_y + vec_wz.y;


	s_dir->DIR_RF = atan2f(vec_rf.y,vec_rf.x)*ANGLE_RANGE/PI;
	s_dir->DIR_RF = (s_dir->DIR_RF + ANGLE_RANGE) % ANGLE_RANGE;

	s_dir->DIR_LF = atan2f(vec_lf.y,vec_lf.x)*ANGLE_RANGE/PI;
	s_dir->DIR_LF = (s_dir->DIR_LF + ANGLE_RANGE) % ANGLE_RANGE;

	s_dir->DIR_LB = atan2f(vec_lb.y,vec_lb.x)*ANGLE_RANGE/PI;
	s_dir->DIR_LB = (s_dir->DIR_LB + ANGLE_RANGE) % ANGLE_RANGE;

	s_dir->DIR_RB = atan2f(vec_rb.y,vec_rb.x)*ANGLE_RANGE/PI;
	s_dir->DIR_RB = (s_dir->DIR_RB + ANGLE_RANGE) % ANGLE_RANGE;


	s_wheel->M_RF = sqrtf(vec_rf.x*vec_rf.x + vec_rf.y*vec_rf.y)*60/C_WHEEL;
	s_wheel->M_LF = sqrtf(vec_rf.x*vec_lf.x + vec_rf.y*vec_lf.y)*60/C_WHEEL;
	s_wheel->M_LB = sqrtf(vec_rf.x*vec_lb.x + vec_rf.y*vec_lb.y)*60/C_WHEEL;
	s_wheel->M_RB = sqrtf(vec_rf.x*vec_rb.x + vec_rf.y*vec_rb.y)*60/C_WHEEL;
}

void swerve_forward(chassis_wheels_state_t* s_wheel, chassis_wheels_dir_t* s_dir, chassis_state_t* s_chassis){
	vector_t vec_rf,vec_lf,vec_lb,vec_rb;
	vector_t unit_vec_wz;
	unit_vec_wz.x = cosf(SWERVE_SPIN_ANG*PI/180);
	unit_vec_wz.y = sinf(SWERVE_SPIN_ANG*PI/180);

	vec_rf.x = s_wheel->M_RF*cosf(s_dir->DIR_RF*PI/(ANGLE_RANGE/2))*C_WHEEL/60;
	vec_rf.y = s_wheel->M_RF*sinf(s_dir->DIR_RF*PI/(ANGLE_RANGE/2))*C_WHEEL/60;

	vec_lf.x = s_wheel->M_LF*cosf(s_dir->DIR_LF*PI/(ANGLE_RANGE/2))*C_WHEEL/60;
	vec_lf.y = s_wheel->M_LF*sinf(s_dir->DIR_LF*PI/(ANGLE_RANGE/2))*C_WHEEL/60;

	vec_lb.x = s_wheel->M_LB*cosf(s_dir->DIR_LB*PI/(ANGLE_RANGE/2))*C_WHEEL/60;
	vec_lb.y = s_wheel->M_LB*sinf(s_dir->DIR_LB*PI/(ANGLE_RANGE/2))*C_WHEEL/60;

	vec_rb.x = s_wheel->M_RB*cosf(s_dir->DIR_RB*PI/(ANGLE_RANGE/2))*C_WHEEL/60;
	vec_rb.y = s_wheel->M_RB*sinf(s_dir->DIR_RB*PI/(ANGLE_RANGE/2))*C_WHEEL/60;

	//TODO: vector to chassis state
	s_chassis->v_x = (vec_rf.x + vec_lf.x + vec_lb.x + vec_rb.x)/4;
	s_chassis->v_y = (vec_rf.y + vec_lf.y + vec_lb.y + vec_rb.y)/4;
	s_chassis->w_z = (vec_rf.x * unit_vec_wz.x + vec_rf.y * unit_vec_wz.y)/K_SWERVE*60.0f/(2*PI);

}

#endif

/**
  * @brief update the global varible using motor_data with IIR filter
  * @param[in]  motor_data     pointer to the motor_response_msg_t of 4 wheels,
  			               id and position should match
  * @param[out]  current_wheels pointer to where the caller intended to store the result
  							in the type chassis_wheels_state_t
  * @return void
  */
/*电机数据滤波*/
/*void update_wheels_state(motor_response_msg_t* motor_data, chassis_wheels_state_t* current_wheels){
	current_wheels->M_RF = iir_filter_3((float)motor_data[MOTOR_RF_ID].speed_rpm/REDUCTION_RATIO,MOTOR_RF_ID);
	current_wheels->M_LF = iir_filter_3((float)motor_data[MOTOR_LF_ID].speed_rpm/REDUCTION_RATIO,MOTOR_LF_ID);
	current_wheels->M_LB = iir_filter_3((float)motor_data[MOTOR_LB_ID].speed_rpm/REDUCTION_RATIO,MOTOR_LB_ID);
	current_wheels->M_RB = iir_filter_3((float)motor_data[MOTOR_RB_ID].speed_rpm/REDUCTION_RATIO,MOTOR_RB_ID);
}*/
void update_wheels_state(motor_response_msg_t* motor_data, chassis_wheels_state_t* current_wheels){
	current_wheels->M_RF = iir_filter_3((float)motor_data[MOTOR_RF_ID].speed_rpm*REDUCTION_RATIO,MOTOR_RF_ID);
	current_wheels->M_LF = iir_filter_3((float)motor_data[MOTOR_LF_ID].speed_rpm*REDUCTION_RATIO,MOTOR_LF_ID);
	current_wheels->M_LB = iir_filter_3((float)motor_data[MOTOR_LB_ID].speed_rpm*REDUCTION_RATIO,MOTOR_LB_ID);
	current_wheels->M_RB = iir_filter_3((float)motor_data[MOTOR_RB_ID].speed_rpm*REDUCTION_RATIO,MOTOR_RB_ID);//修改电机减速比为倒数
	
	/*6020角度滤波*/
	current_wheels->DIR_RF = motor_data[STEERING_RF_ID].raw_angle;
	current_wheels->DIR_LF = motor_data[STEERING_LF_ID].raw_angle;
	current_wheels->DIR_LB = motor_data[STEERING_LB_ID].raw_angle;
	current_wheels->DIR_RB = motor_data[STEERING_RB_ID].raw_angle;
	
	/*6020转速滤波*/
	current_wheels->ROT_RF = iir_filter_2(motor_data[STEERING_RF_ID].speed_rpm,0);
    current_wheels->ROT_LF = iir_filter_2(motor_data[STEERING_LF_ID].speed_rpm ,1);
	current_wheels->ROT_LB = iir_filter_2(motor_data[STEERING_LB_ID].speed_rpm ,2);
	current_wheels->ROT_RB = iir_filter_2(motor_data[STEERING_RB_ID].speed_rpm ,3);
}///////待修改

/**
  * @brief set 4 wheels output simutaniously
  * @param  s_wheel: pointer to chassis_wheels_state_t (4 wheel output)
  * @param  max_out: total maximum output, set to >= 4*C620_OUTPUT_MAX to disable
  * @return HAL_OK if transmission was success otherwise HAL_ERROR
  * @attention make sure to limit the output value in (-30000,30000)
  */
/*带功率限制的底盘四轮直接输出函数，直接限制输出峰值*/
HAL_StatusTypeDef chassis_output_ctrl(chassis_wheels_state_t* s_wheel, float max_out){
	motor_transmit_msg_t tx_msg;
extern	float total_output; 
float limit_ratio;
	
	memset(tx_msg.D,0,sizeof(motor_transmit_msg_t));
	
	LIMIT_MIN_MAX(s_wheel->M_LF,-C620_OUTPUT_MAX, C620_OUTPUT_MAX); 
	LIMIT_MIN_MAX(s_wheel->M_RF,-C620_OUTPUT_MAX, C620_OUTPUT_MAX);
	LIMIT_MIN_MAX(s_wheel->M_LB,-C620_OUTPUT_MAX, C620_OUTPUT_MAX);
	LIMIT_MIN_MAX(s_wheel->M_RB,-C620_OUTPUT_MAX, C620_OUTPUT_MAX);
	total_output = fabsf(s_wheel->M_LF) + fabsf(s_wheel->M_RF) + fabsf(s_wheel->M_LB) + fabsf(s_wheel->M_RB);
	if(total_output > max_out){
		limit_ratio = max_out / total_output;
		s_wheel->M_LF *= limit_ratio; 
    s_wheel->M_RF *= limit_ratio;
		s_wheel->M_LB *= limit_ratio; 
    s_wheel->M_RB *= limit_ratio;
	}
	tx_msg.D[MOTOR_LF_ID] = (int16_t)s_wheel->M_LF; 
  tx_msg.D[MOTOR_RF_ID] = (int16_t)s_wheel->M_RF;
	tx_msg.D[MOTOR_LB_ID] = (int16_t)s_wheel->M_LB; 
  tx_msg.D[MOTOR_RB_ID] = (int16_t)s_wheel->M_RB;
	return set_motor_output(&(CHASSIS_MOTORS_HCAN),&tx_msg,C620_ID_BASE);
}

/**
  * @brief chassis motion control process
  * @param[in]  pPID           pointer to the pid_struct_t of 4 wheels speed
  * @param[in]  current_state  pointer to chassis_wheels_state_t (4 wheel speed)
  * @param[in]  ref_state      expected v_x,v_y,w_z in chassis_state_t
  * @param[in]  max_out        total maximum output, set to >= 4*C620_OUTPUT_MAX to disable
  * @return HAL_OK if transmission was success otherwise HAL_ERROR
  * @attention make sure to limit the output value in (-30000,30000)
  */
/*带功率限制的四轮PID控制底盘*/
 steering_angleFdb angleFdb;
//motor_transmit_msg_t p_msg;
//motor_transmit_msg_t p_msg1;
int16_t delta_ang_RFsteering,delta_ang_LFsteering,delta_ang_LBsteering,delta_ang_RBsteering;
HAL_StatusTypeDef chassis_pid_ctrl(pid_struct_t pPID[4],steering_pid_struct_t sPID[4][2],
					  chassis_wheels_state_t* current_state, 
					  chassis_state_t* ref_state,
						chassis_power_lim_t* pow_lim){


	motor_transmit_msg_t tx_msg;
	motor_transmit_msg_t tx_msg1;						
	float  limit_ratio,tmp[4],tmp1[4];
	
	memset(tx_msg.D,0,sizeof(motor_transmit_msg_t));
	memset(tx_msg1.D,0,sizeof(motor_transmit_msg_t));
							
	chassis_wheels_state_t wheels_ref = steeringMotor_reverse(ref_state);
							
							
	streeingFdb_data[0]= ((current_state->DIR_RF - streeingZero[0]) / 8191.f) * 360.f; //0--360
	streeingFdb_data[1]= ((current_state->DIR_LF - streeingZero[1]) / 8191.f) * 360.f; //0--360
	streeingFdb_data[3]= ((current_state->DIR_RB - streeingZero[3]) / 8191.f) * 360.f; //0--360
	streeingFdb_data[2]= ((current_state->DIR_LB - streeingZero[2]) / 8191.f) * 360.f; //0--360	
	
	angleFdb.DIR_RF =streeingFdb_data[0];
	angleFdb.DIR_LF =streeingFdb_data[1];
	angleFdb.DIR_RB =streeingFdb_data[3];
	angleFdb.DIR_LB= streeingFdb_data[2];					
							
		/*设定值转化为当前角度圈数下的设定值*/ //-180---180°
	if (ABS(wheels_ref.DIR_RF -angleFdb.DIR_RF ) > 180.f)
	{
		if (wheels_ref.DIR_RF > angleFdb.DIR_RF)
		{
		wheels_ref.DIR_RF -= 360.f;
		}
		else if (wheels_ref.DIR_RF <angleFdb.DIR_RF)
		{
			wheels_ref.DIR_RF += 360.f;
		}
	}
	/*设定值转化为当前角度圈数下的设定值*/ //-180---180°
	if (ABS(wheels_ref.DIR_LF -angleFdb.DIR_LF) > 180.f)
	{
		if (wheels_ref.DIR_LF > angleFdb.DIR_LF)
		{
		wheels_ref.DIR_LF -= 360.f;
		}
		else if (wheels_ref.DIR_LF <angleFdb.DIR_LF)
		{
			wheels_ref.DIR_LF += 360.f;
		}
	}
	/*设定值转化为当前角度圈数下的设定值*/ //-180---180°
		if (ABS(wheels_ref.DIR_LB - angleFdb.DIR_LB) > 180.f)
	{
		if (wheels_ref.DIR_LB > angleFdb.DIR_LB)
		{
		wheels_ref.DIR_LB -= 360.f;
		}
		else if (wheels_ref.DIR_LB <angleFdb.DIR_LB)
		{
			wheels_ref.DIR_LB += 360.f;
		}
	}
	/*设定值转化为当前角度圈数下的设定值*/ //-180---180°
		if (ABS(wheels_ref.DIR_RB - angleFdb.DIR_RB) > 180.f)
	{
		if (wheels_ref.DIR_RB > angleFdb.DIR_RB)
		{
		wheels_ref.DIR_RB -= 360.f;
		}
		else if (wheels_ref.DIR_RB <angleFdb.DIR_RB)
		{
			wheels_ref.DIR_RB += 360.f;
		}
	}



	
	tmp[MOTOR_RF_ID] = pid_calc(&pPID[MOTOR_RF_ID], wheels_ref.M_RF,current_state->M_RF);
	tmp[MOTOR_LF_ID] = pid_calc(&pPID[MOTOR_LF_ID], wheels_ref.M_LF,current_state->M_LF);
	tmp[MOTOR_LB_ID] = pid_calc(&pPID[MOTOR_LB_ID], wheels_ref.M_LB,current_state->M_LB);
	tmp[MOTOR_RB_ID] = pid_calc(&pPID[MOTOR_RB_ID], wheels_ref.M_RB,current_state->M_RB);
	/*未测试舵轮输出*/						
//	tmp1[(STEERING_RF_ID+CAN_1_2_DIV) % 0x04] = steering_pid_calc(&sPID[(STEERING_RF_ID+CAN_1_2_DIV) % 0x04], wheels_ref.DIR_RF,(current_state->DIR_RF)/4096*180);
//  tmp1[(STEERING_LF_ID+CAN_1_2_DIV) % 0x04] = steering_pid_calc(&sPID[(STEERING_LF_ID+CAN_1_2_DIV) % 0x04], wheels_ref.DIR_LF,(current_state->DIR_LF)/4096*180);
//	tmp1[(STEERING_LB_ID+CAN_1_2_DIV) % 0x04] = steering_pid_calc(&sPID[(STEERING_LB_ID+CAN_1_2_DIV) % 0x04], wheels_ref.DIR_LB,(current_state->DIR_LB)/4096*180);
//	tmp1[(STEERING_RB_ID+CAN_1_2_DIV) % 0x04] = steering_pid_calc(&sPID[(STEERING_RB_ID+CAN_1_2_DIV) % 0x04], wheels_ref.DIR_RB,(current_state->DIR_RB)/4096*180);
//  
	int16_t RF_ref_ang,LF_ref_ang,LB_ref_ang,RB_ref_ang;

  	RF_ref_ang=wheels_ref.DIR_RF*4096/180;
	LF_ref_ang=wheels_ref.DIR_LF*4096/180;
	LB_ref_ang=wheels_ref.DIR_LB*4096/180;
	RB_ref_ang=wheels_ref.DIR_RB*4096/180;
	
//  if (RF_ref_ang < -ANGLE_RANGE / 2) 
//		RF_ref_ang += ANGLE_RANGE; // 取短路径 
//				else if (RF_ref_ang> ANGLE_RANGE / 2) 
//					RF_ref_ang -= ANGLE_RANGE;
//				
//	if (LF_ref_ang < -ANGLE_RANGE / 2) 
//		LF_ref_ang += ANGLE_RANGE; // 取短路径 
//				else if (LF_ref_ang> ANGLE_RANGE / 2) 
//					LF_ref_ang -= ANGLE_RANGE;			
//				
//	if (LB_ref_ang < -ANGLE_RANGE / 2) 
//		LB_ref_ang += ANGLE_RANGE; // 取短路径 
//				else if (LB_ref_ang> ANGLE_RANGE / 2) 
//					LB_ref_ang -= ANGLE_RANGE;
//	if (RB_ref_ang < -ANGLE_RANGE / 2) 
//		RB_ref_ang += ANGLE_RANGE; // 取短路径 
//				else if (RB_ref_ang> ANGLE_RANGE / 2) 
//					RB_ref_ang -= ANGLE_RANGE;		

	delta_ang_RFsteering = get_delta_ang( angleFdb.DIR_RF*4096/180,RF_ref_ang);
	delta_ang_LFsteering = get_delta_ang( angleFdb.DIR_LF*4096/180,LF_ref_ang);	
	delta_ang_LBsteering = get_delta_ang( angleFdb.DIR_LB*4096/180,LB_ref_ang);	
	delta_ang_RBsteering = get_delta_ang( angleFdb.DIR_RB*4096/180,RB_ref_ang);	
				

	steering_output[0]  =  (int16_t)steering_pid_dual_loop(sPID[(STEERING_RF_ID+CAN_1_2_DIV) % 0x04], RF_ref_ang,angleFdb.DIR_RF*4096/180,current_state->ROT_RF );
	steering_output[1]  =  (int16_t)steering_pid_dual_loop(sPID[(STEERING_LF_ID+CAN_1_2_DIV) % 0x04], LF_ref_ang,angleFdb.DIR_LF*4096/180,current_state->ROT_LF );
	steering_output[2]  =  (int16_t)steering_pid_dual_loop(sPID[(STEERING_LB_ID+CAN_1_2_DIV) % 0x04], LB_ref_ang,angleFdb.DIR_LB*4096/180,current_state->ROT_LB );
	steering_output[3]  =  (int16_t)steering_pid_dual_loop(sPID[(STEERING_RB_ID+CAN_1_2_DIV) % 0x04], RB_ref_ang,angleFdb.DIR_RB*4096/180,current_state->ROT_RB );
 
	tmp1[(STEERING_RF_ID+CAN_1_2_DIV) % 0x04]=steering_output[0]  ;
	tmp1[(STEERING_LF_ID+CAN_1_2_DIV) % 0x04]=steering_output[1] ;
	tmp1[(STEERING_LB_ID+CAN_1_2_DIV) % 0x04]=steering_output[2] ;
	tmp1[(STEERING_RB_ID+CAN_1_2_DIV) % 0x04]=steering_output[3] ;
 
//tmp1[(STEERING_RF_ID+CAN_1_2_DIV) % 0x04] = (int16_t)steering_pid_dual_loop(sPID[(STEERING_RF_ID+CAN_1_2_DIV) % 0x04], delta_ang_RFsteering,current_state->ROT_RF );
//tmp1[(STEERING_LF_ID+CAN_1_2_DIV) % 0x04] = (int16_t)steering_pid_dual_loop(sPID[(STEERING_LF_ID+CAN_1_2_DIV) % 0x04], delta_ang_LFsteering,current_state->ROT_LF );
//tmp1[(STEERING_LB_ID+CAN_1_2_DIV) % 0x04] = (int16_t)steering_pid_dual_loop(sPID[(STEERING_LB_ID+CAN_1_2_DIV) % 0x04], delta_ang_LBsteering,current_state->ROT_LB );
//tmp1[(STEERING_RB_ID+CAN_1_2_DIV) % 0x04] = (int16_t)steering_pid_dual_loop(sPID[(STEERING_RB_ID+CAN_1_2_DIV) % 0x04], delta_ang_RBsteering,current_state->ROT_RB );
//	total_output = fabsf(tmp[MOTOR_RF_ID]) + fabsf(tmp[MOTOR_LF_ID]) 
//				+ fabsf(tmp[MOTOR_LB_ID]) + fabsf(tmp[MOTOR_RB_ID]);
//	if(total_output > pow_lim->max_total_out){
//		limit_ratio = pow_lim->max_total_out / total_output;
//	}else{
//		limit_ratio = 1.0f;
//	}

	limit_ratio = 1.0f;
	if(pow_lim->vxy_ratio < 1) pow_lim->vxy_ratio = 1;
	

	tx_msg.D[MOTOR_RF_ID] = (int16_t)((tmp[MOTOR_RF_ID] * limit_ratio) / pow_lim->vxy_ratio);
	tx_msg.D[MOTOR_LF_ID] = (int16_t)((tmp[MOTOR_LF_ID] * limit_ratio) / pow_lim->vxy_ratio);
	tx_msg.D[MOTOR_LB_ID] = (int16_t)((tmp[MOTOR_LB_ID] * limit_ratio) / pow_lim->vxy_ratio);
	tx_msg.D[MOTOR_RB_ID] = (int16_t)((tmp[MOTOR_RB_ID] * limit_ratio) / pow_lim->vxy_ratio);

//memcpy(&p_msg,&tx_msg,sizeof (motor_transmit_msg_t));

	set_motor_output(&(CHASSIS_MOTORS_HCAN),&tx_msg,C620_ID_BASE);
	//memset(tx_msg.D,0,sizeof(motor_transmit_msg_t));
	
	tx_msg1.D[(STEERING_RF_ID-CAN_1_2_DIV) % 0x04] = (int16_t)((tmp1[(STEERING_RF_ID+CAN_1_2_DIV) % 0x04] * limit_ratio) / pow_lim->vxy_ratio);
	tx_msg1.D[(STEERING_LF_ID-CAN_1_2_DIV) % 0x04] = (int16_t)((tmp1[(STEERING_LF_ID+CAN_1_2_DIV) % 0x04] * limit_ratio) / pow_lim->vxy_ratio);
	tx_msg1.D[(STEERING_LB_ID-CAN_1_2_DIV) % 0x04] = (int16_t)((tmp1[(STEERING_LB_ID+CAN_1_2_DIV) % 0x04] * limit_ratio) / pow_lim->vxy_ratio);
	tx_msg1.D[(STEERING_RB_ID-CAN_1_2_DIV) % 0x04] = (int16_t)((tmp1[(STEERING_RB_ID+CAN_1_2_DIV) % 0x04] * limit_ratio) / pow_lim->vxy_ratio);


//memcpy(&p_msg1,&tx_msg1,sizeof (motor_transmit_msg_t));		
	 set_motor_output(&(STEERING_MOTORS_HCAN),&tx_msg1,M6020_ID_BASE);//////呃呃呃set_motor_output(&(CHASSIS_MOTORS_HCAN),&tx_msg,C620_ID_BASE);
	return HAL_OK;
}////////待修改

#if 0
extern TIM_HandleTypeDef CHASSIS_MOTORS_TIM;
struct {
  BrcBd_port_t can_port;
  uint16_t stdid;
  motor_response_msg_t data;
} Motor_On_BrcBd_CAN[10] = {0}; // 来自从机
struct {
  uint16_t stdid;
  motor_transmit_msg_t tx_msg;
} BrcBd_CAN_TX_test = {0}; // 发往从机

//HAL_StatusTypeDef BrcBd_init_flag = HAL_ERROR;
//bool IDLE_FLAG = false;

uint8_t Motor_ID = 0;
bool Enable_Motor = false;
motor_transmit_msg_t set_rpm ;
motor_transmit_msg_t set1_rpm ;
float kp = 3;
float pid_out = 0;


void BrcBd_can1_htimCallback() {
	memcpy(&set_rpm,&p_msg,sizeof (motor_transmit_msg_t));
	if(chassis_pid_ctrl==HAL_OK){

	for (int i = 0; i < 8; i++) {
    // 1khz
    /**********测试通过拓展板接收信号***************/
#define can_pack BrcBd_S2H_data.can_packs[i]
    // 几号端口
    Motor_On_BrcBd_CAN[i].can_port = can_pack
	
	.CONN.port;
    // ID多少
    Motor_On_BrcBd_CAN[i].stdid = can_pack.id;
    // 电机数据
    uint8_t *raw_can_pack = (uint8_t *)&can_pack.data;
    Motor_On_BrcBd_CAN[i].data.raw_angle =
        ((raw_can_pack[0] << 8) | raw_can_pack[1]);
    Motor_On_BrcBd_CAN[i].data.speed_rpm =
        ((raw_can_pack[2] << 8) | raw_can_pack[3]);
    Motor_On_BrcBd_CAN[i].data.current =
        ((raw_can_pack[4] << 8) | raw_can_pack[5]);
    Motor_On_BrcBd_CAN[i].data.tempture = raw_can_pack[6];

    /**********测试通过拓展板发送信号***************/

    // 获得发送stdid及位置
    Motor_ID = Motor_On_BrcBd_CAN[i].stdid - 0x200;
    BrcBd_CAN_TX_test.stdid = Motor_ID <= 4 ? 0x200 : 0x1FF;
    if (Enable_Motor == true) {
      // PID闭环设定电流
      pid_out = kp * (set_rpm.D[i] - Motor_On_BrcBd_CAN[i].data.speed_rpm);
      BrcBd_CAN_TX_test.tx_msg.D[Motor_ID % 5 - 1] = pid_out;
    }
    // 发送
    uint8_t tx_buffer[BrcBd_CAN_DAT_LEN];
    uint32_t *ptx_msg = (uint32_t *)&BrcBd_CAN_TX_test.tx_msg.D[0];
    *(uint32_t *)(&tx_buffer[0]) = __REV16(ptx_msg[0]);
    *(uint32_t *)(&tx_buffer[4]) = __REV16(ptx_msg[1]);
    BrcBd_Transmit_Data_Scheduler(can_conn, Motor_On_BrcBd_CAN[i].can_port,
                                  BrcBd_CAN_TX_test.stdid, tx_buffer);
    memset(&BrcBd_CAN_TX_test.tx_msg, 0, sizeof(motor_transmit_msg_t));
    // 务必要清零
    // BrcBd_Transmit_Data_Scheduler(other)...
    BrcBd_Transmit_Data_Distributor();
	} 
}
}

void BrcBd_can2_htimCallback() {

	memcpy(&set1_rpm,&p_msg1,sizeof (motor_transmit_msg_t));
	if(chassis_pid_ctrl==HAL_OK){

	for (int i = 0; i>3&&i < 8; i++) {
    // 1khz
    /**********测试通过拓展板接收信号***************/
#define can_pack BrcBd_S2H_data.can_packs[i]
    // 几号端口
    Motor_On_BrcBd_CAN[i].can_port = can_pack
	
	.CONN.port;
    // ID多少
    Motor_On_BrcBd_CAN[i].stdid = can_pack.id;
    // 电机数据
    uint8_t *raw_can_pack = (uint8_t *)&can_pack.data;
    Motor_On_BrcBd_CAN[i].data.raw_angle =
        ((raw_can_pack[0] << 8) | raw_can_pack[1]);
    Motor_On_BrcBd_CAN[i].data.speed_rpm =
        ((raw_can_pack[2] << 8) | raw_can_pack[3]);
    Motor_On_BrcBd_CAN[i].data.current =
        ((raw_can_pack[4] << 8) | raw_can_pack[5]);
    Motor_On_BrcBd_CAN[i].data.tempture = raw_can_pack[6];

    /**********测试通过拓展板发送信号***************/

    // 获得发送stdid及位置
    Motor_ID = Motor_On_BrcBd_CAN[i].stdid - 0x200;
    BrcBd_CAN_TX_test.stdid = Motor_ID <= 4 ? 0x200 : 0x1FF;
    if (Enable_Motor == true) {
      // PID闭环设定电流
      pid_out = kp * (set1_rpm.D[i] - Motor_On_BrcBd_CAN[i].data.speed_rpm);
      BrcBd_CAN_TX_test.tx_msg.D[Motor_ID % 5 - 1] = pid_out;
    }
    // 发送
    uint8_t tx_buffer[BrcBd_CAN_DAT_LEN];
    uint32_t *ptx_msg = (uint32_t *)&BrcBd_CAN_TX_test.tx_msg.D[0];
    *(uint32_t *)(&tx_buffer[0]) = __REV16(ptx_msg[0]);
    *(uint32_t *)(&tx_buffer[4]) = __REV16(ptx_msg[1]);
    BrcBd_Transmit_Data_Scheduler(can_conn, Motor_On_BrcBd_CAN[i].can_port,
                                  BrcBd_CAN_TX_test.stdid, tx_buffer);
    memset(&BrcBd_CAN_TX_test.tx_msg, 0, sizeof(motor_transmit_msg_t));
    // 务必要清零
    // BrcBd_Transmit_Data_Scheduler(other)...
    BrcBd_Transmit_Data_Distributor();
	} 
}
}
#endif

/*HAL_StatusTypeDef chassis_pid_ctrl(pid_struct_t pPID[4],
					  chassis_wheels_state_t* current_state, 
					  chassis_state_t* ref_state,
						chassis_power_lim_t* pow_lim){
	motor_transmit_msg_t tx_msg;
	float total_output, limit_ratio,tmp[4];
	
	memset(tx_msg.D,0,sizeof(motor_transmit_msg_t));

	chassis_wheels_state_t wheels_ref = omni_reverse(ref_state);

	tmp[MOTOR_RF_ID] = pid_calc(&pPID[MOTOR_RF_ID], wheels_ref.M_RF,current_state->M_RF);
	tmp[MOTOR_LF_ID] = pid_calc(&pPID[MOTOR_LF_ID], wheels_ref.M_LF,current_state->M_LF);
	tmp[MOTOR_LB_ID] = pid_calc(&pPID[MOTOR_LB_ID], wheels_ref.M_LB,current_state->M_LB);
	tmp[MOTOR_RB_ID] = pid_calc(&pPID[MOTOR_RB_ID], wheels_ref.M_RB,current_state->M_RB);

	total_output = fabsf(tmp[MOTOR_RF_ID]) + fabsf(tmp[MOTOR_LF_ID]) + fabsf(tmp[MOTOR_LB_ID]) + fabsf(tmp[MOTOR_RB_ID]);
	if(total_output > 4*C620_OUTPUT_MAX){
		limit_ratio = 4*C620_OUTPUT_MAX / total_output;
	}else{
		limit_ratio = 1.0f;
	}
	tx_msg.D[MOTOR_RF_ID] = (int16_t)((tmp[MOTOR_RF_ID] * limit_ratio) / pow_lim->vxy_ratio);
	tx_msg.D[MOTOR_LF_ID] = (int16_t)((tmp[MOTOR_LF_ID] * limit_ratio) / pow_lim->vxy_ratio);
	tx_msg.D[MOTOR_LB_ID] = (int16_t)((tmp[MOTOR_LB_ID] * limit_ratio) / pow_lim->vxy_ratio);
	tx_msg.D[MOTOR_RB_ID] = (int16_t)((tmp[MOTOR_RB_ID] * limit_ratio) / pow_lim->vxy_ratio);
	

	return set_motor_output(&(CHASSIS_MOTORS_HCAN),&tx_msg,C620_ID_BASE);
	
}*/

/**
  * @brief  chassis motion control process in another algorithm
  * @param[in]  pPID           pointer to the pid_struct_t of 3 chassis state speed
  * @param[in]  current_state  pointer to chassis_wheels_state_t (4 wheel speed)
  * @param[in]  ref_state      expected v_x,v_y,w_z in chassis_state_t
  * @param[in]  pow_lim        power limit parameters
  * @return HAL_OK if transmission was success otherwise HAL_ERROR
  * @attention make sure to limit the output value in (-30000,30000)
  */
/*带功率限制的车速PID控制底盘*/
HAL_StatusTypeDef chassis_pid_revctrl(pid_struct_t pPID[3],
					  chassis_wheels_state_t* current_state, 
					  chassis_state_t* ref_state, 
					  chassis_power_lim_t* pow_lim){

	chassis_wheels_state_t wheels_output;
	chassis_state_t motion_output, motion_current;

	motion_current = mecanum_forward(current_state);
	
	pPID[0].out_max = pow_lim->vxy_output_max;
	pPID[1].out_max = pow_lim->vxy_output_max;
	pPID[2].out_max = pow_lim->wz_output_max;
						  
	motion_output.v_x = (int16_t)pid_calc_deadband(&pPID[0], ref_state->v_x,motion_current.v_x);
	motion_output.v_y = (int16_t)pid_calc_deadband(&pPID[1], ref_state->v_y,motion_current.v_y);
	motion_output.w_z = (int16_t)pid_calc(&pPID[2], ref_state->w_z,motion_current.w_z);

	if(pow_lim->vxy_ratio < 1) pow_lim->vxy_ratio = 1;
	if(pow_lim->wz_ratio < 1) pow_lim->wz_ratio = 1;

	motion_output.v_x /= pow_lim->vxy_ratio;
	motion_output.v_y /= pow_lim->vxy_ratio;
	motion_output.w_z /= pow_lim->wz_ratio;

	wheels_output = mecanum_reverse(&motion_output);
	return chassis_output_ctrl(&wheels_output, pow_lim->max_total_out);

}

#endif
